#include "augmentOpencv.h"
#include <vector>

CvScalarI cvScalarI(uchar* ptr) {
	CvScalarI res;
	res.val = ptr;
	for (int i=0; i<3; ++i, ++ptr)
		res.data[i] = *ptr;
	return res;
}
CvScalarI cvScalarI(void) {
	CvScalarI res;
	res.val = NULL;
	memset(res.data, 0, 3 * sizeof(int));
	return res;
}
CvScalarI cvScalarI(CvScalar s) {
	CvScalarI res;
	for (int i=0; i<3; ++i)
		res.data[i] = int(s.val[i]);
	res.val = NULL;
	return res;
}

void cvShowRGB(CvScalar sca) {
	cout << sca.val[2] << " " << sca.val[1] << " " << sca.val[0] << endl;
}

void printMat(char varName[], cv::Mat mat) {
	CvMat newmat = mat;
	cvPrintMat(varName, &newmat);
}


void cvPrintMat(char variableName[], CvMat* mat, ostream* out, bool bgr) {
	(*out) << variableName << ":" << endl;
	cvPrintMat(mat, out, bgr);
}

void cvPrintMat(CvMat* mat, ostream* out, bool bgr) {
	int cn = CV_MAT_CN(mat->type);
	for (int i=0; i<mat->rows; ++i) {
		for (int j=0; j<mat->cols; ++j) {
			CvScalar sca = cvGet2D(mat, i, j);
			if (cn == 3) {
				if (bgr)
					(*out) << "\t(" << sca.val[2] << ", " << sca.val[1] << ", " << sca.val[0] << ")";
				else
					(*out) << "\t(" << sca.val[0] << ", " << sca.val[1] << ", " << sca.val[2] << ")";
			}
			else {
				(*out) << "\t" << sca.val[0];
			}
		}
		(*out) << endl;
	}
}

void cvPrintMat(CvMat* mat, char filename[], bool bgr) {
	ofstream fout;
	fout.open(filename);
	cvPrintMat(mat, &fout, bgr);
	fout.close();
}

CvMat *cvLoadMat(char filename[], int cn, bool isUchar) {
	static char line[100000];
	CvMat *ret;
	FILE *inFile;
	fopen_s(&inFile, filename, "r");
	if (isUchar) {
		vector<uchar> data;
		uchar val;
		int rows = 0, cols = 0;
		while (fgets(line, 100000, inFile) != NULL) {
			istringstream ins(line);
			if (cn == 1) {
				while (ins >> val) {
					if (rows == 0) ++cols;
					data.push_back(val);
				}
			}
			else {
				cout << "didn't handle. loadMat failed." << endl;
				cols /= cn;
			}
			++rows;
		}
		ret = cvCreateMat(rows, cols, CV_MAKE_TYPE(CV_8U, cn));
		uchar *ptr = ret->data.ptr;
		for (vector<uchar>::iterator it=data.begin(); it!=data.end(); ++it)
			*ptr = *it;
	}
	else {
		vector<float> data;
		float val;
		int rows = 0, cols = 0;
		while (fgets(line, 100000, inFile) != NULL) {
			istringstream ins(line);
			int cnt = 0;
			if (cn == 1) {
				while (ins >> val) {
					++cnt;
					//					ins >> val;
					//					if (ins.flags() == ios::failbit);
					if (rows == 0) ++cols;
					data.push_back(val);
				}
				if (cnt < 800) {
					cout << rows << " " << cnt << endl;
					int debug;
					debug = 1;
					cout << ins.flags() << endl;
				}
			}
			else {
				cout << "didn't handle. loadMat failed." << endl;
				cols /= cn;
			}
			++rows;
		}
		cout << rows << " " << cols << endl;
		ret = cvCreateMat(rows, cols, CV_MAKE_TYPE(CV_32F, cn));
		float *ptr = ret->data.fl;
		CvScalar sca;
		vector<float>::iterator it=data.begin();
		int dataPtr = 0;
		for (int i=0; i<rows; ++i)
			for (int j=0; j<cols; ++j) {
				for (int k=0; k<cn; ++k, ++dataPtr)
					sca.val[k] = data[dataPtr];
				cvSet2D(ret, i, j, sca);
			}
	}

	return ret;
}

void debugByMouseDb(int type, int x, int y, int flag, void* param) {
	switch (type) {
		case CV_EVENT_LBUTTONDOWN:
			CvMat* mat = (CvMat*) param;
			printf("point at (%d, %d) = %lf\n", y, x, CV_MAT_ELEM(*mat, double, y, x));
			break;
	}
}

void debugByMouseCh(int type, int x, int y, int flag, void* param) {
	switch (type) {
		case CV_EVENT_LBUTTONDOWN:
			CvMat* mat = (CvMat*) param;
			if (CV_MAT_CN(mat->type) == 1)
				printf("point at (%d, %d) = %d\n", y, x, CV_MAT_ELEM(*mat, uchar, y, x));
			else
				cout << "point at (" << y << ", " << x << ") = ";
			cvShowRGB(cvGet2D(mat, y, x));
			break;
	}
}

CvMat* cvRgb2gray(CvMat* img) {
	CvMat *gray;
	gray = cvCreateMat(img->rows, img->cols, CV_8UC1);
	cvCvtColor(img, gray, CV_BGR2GRAY);
	return gray;
}

template<typename T>
inline void divide2(double *des, T *src, int size, double b) {
	for (int i=0; i<size; ++i,++des,++src)
		*des = *src / b;
}

//only return double
CvMat* cvDivide (CvMat *a, double b) {
	CvMat* res;
	int cn = CV_MAT_CN(a->type);
	res = cvCreateMat(a->rows, a->cols, CV_MAKETYPE(CV_64F, cn));
	double *desp = res->data.db;
	int size = a->rows*a->cols*cn;
	void *srcp = NULL;
	switch (CV_MAT_DEPTH(a->type)) {
		case 0:
		case 1:
			srcp = a->data.ptr;
			divide2(desp, (uchar*)srcp, size, b);
			break;
		case 2:
		case 3:
			srcp = a->data.s;
			divide2(desp, (ushort*)srcp, size, b);
			break;
		case 4:
			srcp = a->data.i;
			divide2(desp, (int*)srcp, size, b);
			break;
		case 5:
			srcp = a->data.fl;
			divide2(desp, (float*)srcp, size, b);
			break;
		case 6:
			srcp = a->data.db;
			divide2(desp, (double*)srcp, size, b);
			break;
	}
	return res;
}

template<typename T>
inline void multi2(uchar *des, T *src, int size, const uchar scale) {
	for (int i=0; i<size; ++i,++des,++src)
		*des = (uchar)(*src * scale);
}
//only return uchar
CvMat* cvMul2 (const uchar scale, const CvMat* src) {
	CvMat* res = NULL;
	int cn = CV_MAT_CN(src->type);
	res = cvCreateMat(src->rows, src->cols, CV_MAKETYPE(CV_8U, cn));
	int size = src->rows*src->cols*cn;
	void *srcp = NULL;
	switch (CV_MAT_DEPTH(src->type)) {
		case CV_8U:
		case CV_8S:
			srcp = src->data.ptr;
			multi2(res->data.ptr, (uchar*)srcp, size, scale);
			break;
		case CV_16U:
		case CV_16S:
			srcp = src->data.s;
			multi2(res->data.ptr, (ushort*)srcp, size, scale);
			break;
		case CV_32S:
			srcp = src->data.i;
			multi2(res->data.ptr, (int*)srcp, size, scale);
			break;
		case CV_32F:
			srcp = src->data.fl;
			multi2(res->data.ptr, (float*)srcp, size, scale);
			break;
		case CV_64F:
			srcp = src->data.db;
			multi2(res->data.ptr, (double*)srcp, size, scale);
			break;
	}
	return res;
}

//only return double
CvMat* cvMul2(CvMat* inSrc1, CvMat* inSrc2, CvMat *dst, int retType) {
	CvMat* res = NULL;
	CvMat *src1, *src2;
	int cn1 = CV_MAT_CN(inSrc1->type);
	int cn2 = CV_MAT_CN(inSrc2->type);
	src1 = cvCreateMat(inSrc1->rows, inSrc1->cols, CV_MAKETYPE(retType, cn1));
	src2 = cvCreateMat(inSrc2->rows, inSrc2->cols, CV_MAKETYPE(retType, cn2));
	cvConvert(inSrc1, src1);
	cvConvert(inSrc2, src2);

	if (dst == NULL)
		res = cvCreateMat(src1->rows, src2->cols, CV_MAKETYPE(retType, max(cn1, cn2)));
	else
		res = dst;
	if (cn1 == cn2)
		cvMul(src1, src2, res);
	else {
		CvMat *ch1, *ch2, *ch3;
		CvMat *resCh1, *resCh2, *resCh3;
		if (cn1 == 1) {
			ch1 = cvCreateMat(src2->rows, src2->cols, retType);
			ch2 = cvCreateMat(src2->rows, src2->cols, retType);
			ch3 = cvCreateMat(src2->rows, src2->cols, retType);
			cvSplit(src2, ch1, ch2, ch3, NULL);
			resCh1 = cvCreateMat(src2->rows, src2->cols, retType);
			resCh2 = cvCreateMat(src2->rows, src2->cols, retType);
			resCh3 = cvCreateMat(src2->rows, src2->cols, retType);
			cvMul(src1, ch1, resCh1);
			cvMul(src1, ch2, resCh2);
			cvMul(src1, ch3, resCh3);
			cvMerge(resCh1, resCh2, resCh3, NULL, res);
		}
		else {
			ch1 = cvCreateMat(src1->rows, src1->cols, retType);
			ch2 = cvCreateMat(src1->rows, src1->cols, retType);
			ch3 = cvCreateMat(src1->rows, src1->cols, retType);
			cvSplit(src1, ch1, ch2, ch3, NULL);
			resCh1 = cvCreateMat(src1->rows, src1->cols, retType);
			resCh2 = cvCreateMat(src1->rows, src1->cols, retType);
			resCh3 = cvCreateMat(src1->rows, src1->cols, retType);
			cvMul(src2, ch1, resCh1);
			cvMul(src2, ch2, resCh2);
			cvMul(src2, ch3, resCh3);
			cvMerge(resCh1, resCh2, resCh3, NULL, res);
		}
	}
	return res;
}

CvMat* cvIm2double(CvMat* img) {
	CvMat* tmp;
	tmp = cvDivide(img, 255);
	return tmp;
}

CvMat* cvConvertMatType(CvMat* src, int type) {
	CvMat* dest;
	dest = cvCreateMat(src->rows, src->cols, type);
	cvConvert(src, dest);
	return dest;
}
void cvSaveRawImg(char filename[], CvMat *img) {
	FILE* outF;
	fopen_s(&outF, filename, "wb");
	if (CV_MAT_CN(img->type) == 1)
		for (int i=0; i<img->rows; ++i)
			fwrite(CV_MAT_ELEM_PTR(*img, i, 0), CV_ELEM_SIZE(img->type), img->cols, outF);
	else
		for (int i=0; i<img->rows; ++i)
			for (int j=0; j<img->cols; ++j) {
				uchar *ptr = CV_MAT_ELEM_PTR(*img, i, j);
				for (int k=2; k>=0; --k)
					fwrite((ptr+k), CV_ELEM_SIZE(img->type), 1, outF);
			}
			fclose(outF);
}

void cvSaveMatImage(char filename[], CvMat *mat) {
	int cn = CV_MAT_CN(mat->type);
	int imgType = -1;
	CvMat *savMat;
	switch (CV_MAT_TYPE(mat->type)%8) {
	case CV_8U: savMat = mat; imgType = IPL_DEPTH_8U; break;
	case CV_16S: savMat = mat; imgType = IPL_DEPTH_16S; break;
	case CV_32F: 
	case CV_64F: 
		CvMat *tmpMat = cvCreateMat(mat->rows, mat->cols, CV_MAKETYPE(CV_MAT_TYPE(mat->type), cn));
		cvScale(mat, tmpMat, 255);
		savMat = cvConvertMatType(tmpMat, CV_MAKETYPE(CV_8U, cn));
		cvReleaseMat(&tmpMat);
		imgType = IPL_DEPTH_8U;
	}
	IplImage *header = cvCreateImageHeader(cvSize(mat->rows, mat->cols), imgType, cn);
	cvSaveImage(filename, cvGetImage(savMat, header));
}

inline int cvMatElementType(CvMat *src) {return CV_MAT_DEPTH(src->type);}
inline int cvMatType(CvMat *src) {return CV_MAT_TYPE(src->type);}

CvPointListList cvBwlabel(CvMat *src) {
	if (cvMatType(src) != CV_8U) {
		cout << "need CV_8U type in cvBwlabel." << endl;
		throw exception();
	}

	CvPointList** pLists;
	int idx = 0;
	int length[255];
	memset(length, 0, sizeof(int)*255);

	uchar *ptr = src->data.ptr;
	for (int i=0; i<src->rows; ++i)
		for (int j=0; j<src->cols; ++j, ++ptr) {
			switch (*ptr) {
				case 255:
					cvFloodFill(src, cvPoint(j, i), cvScalar(idx+1));
					++length[idx++];
					break;
				case 0:
					continue;
				default:
					++length[*ptr-1];
			}
		}

	pLists = new CvPointList*[idx];
	for (int i=0; i<idx; ++i) {
		pLists[i] = new CvPointList;
		pLists[i]->length = 0;
	}
	ptr = src->data.ptr;
	for (int i=0; i<src->rows; ++i)
		for (int j=0; j<src->cols; ++j, ++ptr) {
			int label = *ptr;
			if (label == 0) continue;
			--label;
			if (pLists[label]->length == 0)
				pLists[label]->data = new CvPoint[length[label]];
			CvPoint *point = new CvPoint;
			*point = cvPoint(i, j);
			pLists[label]->data[pLists[label]->length++] = *point;
		}
	CvPointListList pll;
	pll.length = idx;
	pll.lists = pLists;
	return pll;
}

void cvMinMaxLoc(CvMat *src, CvPointList plist, double &minVal, double &maxVal) {
	int x, y;
	cvMinMaxLoc(src, plist, minVal, maxVal, x, y);
}

void cvMinMaxLoc(CvMat *src, CvPointList plist, double &minVal, double &maxVal,
				 int &minIdx, int &maxIdx) {
	minVal = 100000000;
	maxVal = -100000000;
	double data;
	CvPoint p;
	for (int i=0; i<plist.length; ++i) {
		p = plist.data[i];
		data = cvGetReal2D(src, p.x, p.y);
		if (data < minVal) {
			minVal = data;
			minIdx = i;
		}
		if (data > maxVal) {
			maxVal = data;
			maxIdx = i;
		}
	}
}

char *histName = "histogram";

// void cvShowHist(CvHistogram *pHist, char *winName, int binW) {
// 	if (winName == NULL) winName = histName;
// 
// 	int hsize = pHist->mat.dim[0].size;
// 	CvMat *histImg = cvCreateMat(200, binW*hsize, CV_8UC3);
// 
// 	CvMat *colors = cvCreateMat(1, hsize, CV_8UC3);
// 	for( int i = 0; i < hsize; i++ ) {
// 		uchar *ptr = cvPtr2D(colors, 0, i);
// 		ptr[0] = uchar(i*180./hsize);
// 		ptr[1] = 255;
// 		ptr[2] = 255;
// 	}
// 	cvCvtColor(colors, colors, CV_HSV2BGR);
// 
// 	float maxBinVal;
// 	cvGetMinMaxHistValue(pHist, NULL, &maxBinVal);
// 	if (maxBinVal <= 0.00001) maxBinVal = 200;
// 
// 	for( int i = 0; i < hsize; i++ )
// 	{
// 		float histBinVal = cvQueryHistValue_1D(pHist, i);
// 		int val = cvQueryHistValue_1D(pHist, i) * histImg->rows /maxBinVal;
// 		cvRectangle( histImg, cvPoint(i*binW, histImg->rows),
// 			cvPoint((i+1)*binW, histImg->rows - val),
// 			cvGet2D(colors, 0, i), -1, 8 );
// 	}
// 	cvShowImage(winName, histImg);
// 
// 	cvReleaseMat(&histImg);
// 
// }

// CvHistogram operator - (CvHistogram& a, CvHistogram& b) {
// 	int bins = a.mat.dim[0].size;
// 	CV_Assert(b.mat.dim[0].size == bins);
// 	float *ranges = a.thresh[0];
// 	CvHistogram *sub = cvCreateHist(a.mat.dims, &bins, CV_HIST_ARRAY, &ranges, 1);
// 	for (int i=0; i<bins; ++i) {
// 		*cvGetHistValue_1D(sub, i) = *cvGetHistValue_1D(&a, i) - *cvGetHistValue_1D(&b,i);
// // 		cout << *cvGetHistValue_1D(sub, i) << " "
// // 			<< *cvGetHistValue_1D(&a, i) << " "
// // 			<< *cvGetHistValue_1D(&b, i) << endl;
// 	}
// 	return *sub;
// }

inline int cvFitIn(int x, int a, int b) {return min(max(a,x),b);}

cv::Mat createColor(int cn) {
	cv::Mat colors(cn, 1, CV_8UC3);
	for( int i = 0; i < cn; i++ ) {
		uchar *ptr = colors.ptr<uchar>(i);
		ptr[0] = uchar(i*180./cn);
		ptr[1] = 255;
		ptr[2] = 255;
	}
	cvtColor(colors, colors, CV_HSV2BGR);
	return colors;
}
